/*
 * Copyright  2017-2019 NXP
 * All rights reserved.
 *
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
#include "tos_k.h"
#include "tos_at.h"
#include "cmsis_os.h"
#include "camera_support.h"

#include "pin_mux.h"
#include "board.h"
#include "fsl_debug_console.h"
#include "fsl_lpuart.h"
#include "fsl_pxp.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#include "display_support.h"


// tflite micro
#include "main_functions.h"
//#include "image_provider.h"
#include "tensorflow/lite/micro/cortex_m_generic/debug_log_callback.h"

extern volatile uint8_t* g_framebuffer;
extern int camera_image_width;
extern int camera_image_height;

int person_flag = 0;
int person_count = 0;
int dagerous_flag = 0;

#define USE_PXP_SCALE 0

#define APP_IMG_HEIGHT DEMO_BUFFER_HEIGHT
#define APP_IMG_WIDTH  DEMO_BUFFER_WIDTH
static dc_fb_info_t fbInfo;

#define APP_FRAME_BUFFER_COUNT 4
#define APP_DISP_FRAME_BUFFER_COUNT 2
/* Pixel format RGB565, bytesPerPixel is 2. */
#define APP_BPP 2
#define DISP_BPP 4
#if (FRAME_BUFFER_ALIGN > DEMO_CAMERA_BUFFER_ALIGN)
#define DEMO_FRAME_BUFFER_ALIGN FRAME_BUFFER_ALIGN
#else
#define DEMO_FRAME_BUFFER_ALIGN DEMO_CAMERA_BUFFER_ALIGN
#endif

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
static void APP_BufferSwitchOffCallback(void *param, void *switchOffBuffer);
static void APP_InitCamera(void);
static void APP_InitDisplay(void);
static void APP_CSI_RGB565(void);

/*******************************************************************************
 * Variables
 ******************************************************************************/
AT_NONCACHEABLE_SECTION_ALIGN(
    static uint32_t s_disp_frameBuffer[APP_DISP_FRAME_BUFFER_COUNT][APP_IMG_HEIGHT][APP_IMG_WIDTH],
    DEMO_FRAME_BUFFER_ALIGN);
	
AT_NONCACHEABLE_SECTION_ALIGN(
#if USE_PXP_SCALE
    static uint16_t s_frameBuffer[APP_FRAME_BUFFER_COUNT][DEMO_CAMERA_HEIGHT][DEMO_CAMERA_WIDTH],
    DEMO_FRAME_BUFFER_ALIGN);
#else
static uint16_t s_frameBuffer[APP_FRAME_BUFFER_COUNT][APP_IMG_HEIGHT][APP_IMG_WIDTH],
    DEMO_FRAME_BUFFER_ALIGN);
#endif

#define OPTIMIZE __attribute__((section(".ramfunc.$SRAM_ITC")))
/*
 * When new frame buffer sent to display, it might not be shown immediately.
 * Application could use callback to get new frame shown notification, at the
 * same time, when this flag is set, application could write to the older
 * frame buffer.
 */
static volatile bool s_newFrameShown = false;

static volatile uint8_t s_lcdActiveFbIdx;
/*******************************************************************************
 * Code
 ******************************************************************************/
extern void BOARD_InitCSIPins();

/*!
 * @brief Main function
 */
int display_init(void)
{
	 /* Define the init structure for the output LED pin*/
    gpio_pin_config_t CAM_RST_config = {kGPIO_DigitalOutput, 0, kGPIO_NoIntmode};

    BOARD_EarlyPrepareCamera();
    BOARD_InitCSIPins();
 /* Init output LED GPIO. */
    GPIO_PinInit(GPIO3, 3U, &CAM_RST_config);//��������ͷ��λ����
    GPIO_PinWrite(GPIO3, 3U, 1U);//����ͷ��λ�������ߣ�����ͷ������������
    PRINTF("CSI RGB565 example start...\r\n");

    APP_InitCamera();
	
    APP_InitDisplay();


}



static void APP_InitCamera(void)
{
    const camera_config_t cameraConfig = {
        .pixelFormat   = kVIDEO_PixelFormatRGB565,
        .bytesPerPixel = APP_BPP,
        .resolution    = FSL_VIDEO_RESOLUTION(DEMO_CAMERA_WIDTH, DEMO_CAMERA_HEIGHT),
        /* Set the camera buffer stride according to panel, so that if
         * camera resoution is smaller than display, it can still be shown
         * correct in the screen.
         */
#if USE_PXP_SCALE
        .frameBufferLinePitch_Bytes = DEMO_CAMERA_WIDTH * APP_BPP,
#else
		.frameBufferLinePitch_Bytes = APP_IMG_WIDTH * APP_BPP,
#endif
        .interface                  = kCAMERA_InterfaceGatedClock,
        .controlFlags               = DEMO_CAMERA_CONTROL_FLAGS,
        .framePerSec                = 30,
    };

    memset(s_frameBuffer, 0, sizeof(s_frameBuffer));

    BOARD_InitCameraResource();

    CAMERA_RECEIVER_Init(&cameraReceiver, &cameraConfig, NULL, NULL);

    if (kStatus_Success != CAMERA_DEVICE_Init(&cameraDevice, &cameraConfig))
    {
        PRINTF("Camera device initialization failed\r\n");
        while (1)
        {
            ;
        }
    }
     else{
		  PRINTF("Camera device initialization OK!\r\n");
			PRINTF("Camera START!\r\n");
		 }
    CAMERA_DEVICE_Start(&cameraDevice);

    /* Submit the empty frame buffers to buffer queue. */
    for (uint32_t i = 0; i < APP_FRAME_BUFFER_COUNT; i++)
    {
        CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t)(s_frameBuffer[i]));
    }
}

static void APP_InitDisplay(void)
{

    status_t status;

	BOARD_PrepareDisplayController();
	
    status = g_dc.ops->init(&g_dc);
    if (kStatus_Success != status)
    {
        PRINTF("Display initialization failed\r\n");
        assert(0);
    }
    s_lcdActiveFbIdx = 0;
    g_dc.ops->getLayerDefaultConfig(&g_dc, 0, &fbInfo);
    fbInfo.pixelFormat = kVIDEO_PixelFormatXRGB8888;
    fbInfo.width       = DEMO_BUFFER_WIDTH;
    fbInfo.height      = DEMO_BUFFER_HEIGHT;
    fbInfo.startX      = DEMO_BUFFER_START_Y;
    fbInfo.startY      = DEMO_BUFFER_START_X;
    fbInfo.strideBytes = DEMO_BUFFER_WIDTH * DISP_BPP;
    g_dc.ops->setLayerConfig(&g_dc, 0, &fbInfo);

    g_dc.ops->setCallback(&g_dc, 0, APP_BufferSwitchOffCallback, NULL);

	PXP_Init(PXP);
	PXP_SetProcessSurfaceBackGroundColor(PXP, 0U);
	
	PXP_SetProcessSurfacePosition(PXP, 0U, 0U, DEMO_BUFFER_WIDTH - 1U, DEMO_BUFFER_HEIGHT - 1U);
	PXP_SetAlphaSurfacePosition(PXP,0xffff,0xffff, 0U, 0U);//, DEMO_BUFFER_WIDTH - 1U, DEMO_BUFFER_HEIGHT - 1U);
	PXP_EnableCsc1(PXP, false);
}

static void APP_PXP_CONV_RGB565_To_XRGB8888(uint32_t src, uint32_t dst)
{
	pxp_ps_buffer_config_t src_cfg  = {
		.swapByte    = 0,
		.bufferAddrU = 0U,
		.bufferAddrV = 0U,
		.pixelFormat = kPXP_PsPixelFormatRGB565,
	};
	
	pxp_output_buffer_config_t dst_cfg = {
		.pixelFormat    = kPXP_OutputPixelFormatARGB8888,
		.interlacedMode = kPXP_OutputProgressive,
		.buffer1Addr    = 0U,

	};
	
	/* Clear the whole output buffer. */
    //memset((void*)dst, 0, DEMO_BUFFER_WIDTH*DEMO_BUFFER_HEIGHT*DISP_BPP);
	
	src_cfg.bufferAddr = src;
#if USE_PXP_SCALE
	src_cfg.pitchBytes  = DEMO_CAMERA_WIDTH * APP_BPP;
#else
	src_cfg.pitchBytes  = APP_IMG_WIDTH * APP_BPP;
#endif
	
	dst_cfg.width = APP_IMG_WIDTH;
	dst_cfg.height = APP_IMG_HEIGHT;
	dst_cfg.pitchBytes = APP_IMG_WIDTH*DISP_BPP;
	dst_cfg.buffer0Addr = dst;
	
    PXP_SetProcessSurfaceBufferConfig(PXP, &src_cfg);
#if USE_PXP_SCALE
	PXP_SetProcessSurfaceScaler(PXP,DEMO_CAMERA_WIDTH,DEMO_CAMERA_HEIGHT,APP_IMG_WIDTH,APP_IMG_HEIGHT);
#endif
	PXP_SetOutputBufferConfig(PXP, &dst_cfg);	
	PXP_Start(PXP);
	while (!(kPXP_CompleteFlag & PXP_GetStatusFlags(PXP)))
	{
		//tos_task_delay(1);
	}
	PXP_ClearStatusFlags(PXP, kPXP_CompleteFlag);
}

OPTIMIZE void APP_CONV_RGB565_To_XRGB8888(uint32_t src, uint32_t dst)
{
	int w = DEMO_CAMERA_WIDTH;
	int h = DEMO_CAMERA_HEIGHT;
	uint16_t *srcData = (uint16_t *)src;

	memset((void*)dst, 0, DEMO_BUFFER_WIDTH*DEMO_BUFFER_HEIGHT*DISP_BPP);

	for (int i=0;i<h;i++)
	{
		for (int j=0;j<w;j++)
		{
			uint16_t rgb565 = srcData[i * w + j];
			uint32_t *disp = dst + i*APP_IMG_WIDTH*DISP_BPP+j*DISP_BPP;
			uint8_t r = ((rgb565 >> 11) << 3) & (0xff);
			uint8_t g = ((rgb565 >> 5) << 2) & (0xff);
			uint8_t b = (rgb565 << 3) & (0xff);
			*disp = (r << 16) | (g << 8) | b;
		}
	}
}

static inline void IMAGE_DrawPixel(uint16_t *pDst, uint32_t x, uint32_t y, uint32_t col, uint16_t lcd_w)
{
    pDst[y * (lcd_w) + x] = col;
}

OPTIMIZE void IMAGE_DrawLine(uint16_t* pDst,uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint16_t color, uint32_t lcd_w)
{
    uint32_t x;
    float dx, dy, y, k;
    dx = x1 - x0;
    dy = y1 - y0;
    if (dx != 0)
    {
        k = dy / dx;
        y = y0;
        for (x = x0; x < x1; x++)
        {
            IMAGE_DrawPixel(pDst, x, (uint32_t)(y + 0.5f), color, lcd_w);
            y = y + k;
        }
    }
    else
    {
        x = x0;
        for (y = y0; y < y1; y++)
        {
            IMAGE_DrawPixel(pDst, x, (uint32_t)y, color, lcd_w);
        }
    }
}

OPTIMIZE void IMAGE_DrawRect(uint16_t *pdst, uint32_t x0, uint32_t y0, uint32_t w, uint32_t h, uint16_t color, uint32_t lcd_w)
{
    IMAGE_DrawLine(pdst, x0, y0, x0 + w, y0, color, lcd_w);
    IMAGE_DrawLine(pdst, x0, y0, x0, y0 + h, color, lcd_w);
    IMAGE_DrawLine(pdst, x0, y0 + h, x0 + w, y0 + h, color, lcd_w);
    IMAGE_DrawLine(pdst, x0 + w, y0, x0 + w, y0 + h, color, lcd_w);
}

void debug_log_printf(const char* s)
{
    PRINTF(s);
}

extern void mqttclient_task_send_count(uint32_t count);
static void APP_CSI_RGB565(void)
{
    uint32_t cameraReceivedFrameAddr;
	uint32_t lcdFrameAddr;
	int8_t tflite_output = 0;
	uint32_t count = 0 , count_dagerous = 0;

	s_newFrameShown = false;
	
    //ftlite_micro_initial
    RegisterDebugLogCallback(debug_log_printf);
    setup();

	uint32_t bg_color = 0x00ff0000;
	for (int i=0;i< APP_IMG_HEIGHT;i++)
	{
		for (int j=0;j<APP_IMG_WIDTH;j++)
		{
			s_disp_frameBuffer[0][i][j]= bg_color;
		}
	}

    g_dc.ops->setFrameBuffer(&g_dc, 0, (void *)s_disp_frameBuffer[0]);

    /* For the DBI interface display, application must wait for the first
     * frame buffer sent to the panel.
     */
    if ((g_dc.ops->getProperty(&g_dc) & kDC_FB_ReserveFrameBuffer) == 0)
    {
        while (s_newFrameShown == false)
        {
        	//tos_task_delay(1);
        }
    }

    s_newFrameShown = true;

    g_dc.ops->enableLayer(&g_dc, 0);

	s_newFrameShown = true;
    CAMERA_RECEIVER_Start(&cameraReceiver);
    s_lcdActiveFbIdx = 0;
    TOS_CPU_CPSR_ALLOC();

    camera_image_width = DEMO_CAMERA_WIDTH;
    camera_image_height = DEMO_CAMERA_HEIGHT;

    while (1)
    {
		
        /* Wait to get the full frame buffer to show. */
    	int status = -1;
        while (kStatus_Success != status)
        {
        	TOS_CPU_INT_DISABLE();
        	status = CAMERA_RECEIVER_GetFullBuffer(&cameraReceiver, &cameraReceivedFrameAddr);
        	TOS_CPU_INT_ENABLE();
        	tos_task_delay(1);
        }
        uint32_t start = tos_systick_get();
        //run tflite_micro inference
        g_framebuffer = cameraReceivedFrameAddr;
        loop(&tflite_output);

		uint32_t end = tos_systick_get();
		PRINTF("Time cost: %d \r\n", end - start);


		if(tflite_output == 1){
			GPIO_PinWrite(BOARD_INITPINS_LED_GPIO, BOARD_INITPINS_LED_GPIO_PIN, 1U);
			if(person_flag == 0){
				person_count ++;  //计数统计
				PRINTF("person count: %d \r\n", person_count);
			}
			person_flag = 1;
			count_dagerous++;
			count = 0;
		}else{
			count ++;
			if(count >= 3){ //延迟无人判断
				  GPIO_PinWrite(BOARD_INITPINS_LED_GPIO, BOARD_INITPINS_LED_GPIO_PIN, 0U);
				  count_dagerous = 0;
				  person_flag = 0;
			}
		}

		if(count_dagerous > 5)dagerous_flag = 1;
		else dagerous_flag = 0;

        /* Wait for the previous frame buffer is shown. */
        while (s_newFrameShown == false)
        {
        	tos_task_delay(1);
        }
		lcdFrameAddr                   = (uint32_t)s_disp_frameBuffer[s_lcdActiveFbIdx^1];
		APP_PXP_CONV_RGB565_To_XRGB8888(cameraReceivedFrameAddr,(uint32_t)lcdFrameAddr);
		TOS_CPU_INT_DISABLE();
		CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t)cameraReceivedFrameAddr);
		TOS_CPU_INT_ENABLE();
		s_newFrameShown = false;
		//PRINTF("Add Camera Buffer=0x%x\r\n",cameraReceivedFrameAddr);
		g_dc.ops->setFrameBuffer(&g_dc, 0, (void *)lcdFrameAddr);
		//tos_task_delay(20);
    }
}

static void APP_BufferSwitchOffCallback(void *param, void *switchOffBuffer)
{
		s_newFrameShown = true;
		s_lcdActiveFbIdx ^= 1;
}

void display_entry(void *parameter)
{
	display_init();
	APP_CSI_RGB565();
}
